package com.tango.tangoapp;

import android.content.Intent;
import android.app.Activity;
import android.support.v7.app.AppCompatActivity;
import android.os.Bundle;
import android.util.Log;
import android.view.View;
import android.widget.Button;
import android.widget.TextView;
import android.widget.Toast;

import com.google.atap.tangoservice.Tango;
import com.google.atap.tangoservice.TangoConfig;
import com.google.atap.tangoservice.TangoCoordinateFramePair;
import com.google.atap.tangoservice.TangoErrorException;
import com.google.atap.tangoservice.TangoEvent;
import com.google.atap.tangoservice.TangoInvalidException;
import com.google.atap.tangoservice.TangoOutOfDateException;
import com.google.atap.tangoservice.TangoPointCloudData;
import com.google.atap.tangoservice.TangoPoseData;
import com.google.atap.tangoservice.TangoXyzIjData;
import com.tango.tangoapp.arealearning.AreaLearningActivity;
import com.tango.tangoapp.arealearning.SaveAdfTask;
import com.tango.tangoapp.arealearning.SetAdfNameDialog;
import com.tango.tangoapp.depthperception.OnePointActivity;
import com.tango.tangoapp.depthperception.TwoPointActivity;

import java.util.ArrayList;

public class MainActivity extends Activity{

    private static final String TAG=MainActivity.class.getSimpleName();
    private static final int SECS_TO_MILLISECS=1000;
    private Tango mTango;
    private TangoConfig mConfig;
    private TextView mUuidTextView;
    private TextView mRelocalizationTextView;

    private boolean mIsRelocalized;
    private boolean mIsConstantSpaceRelocalize;

    private double mPreviousPoseTimeStamp;
    private double mTimeToNextUpdate=UPDATE_INTERVAL_MS;

    private static final double UPDATE_INTERVAL_MS=100.0;

    private final Object mSharedLock=new Object();

    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);


        Button btn1, btn2, btn3;
        //set up UI element//

        btn1 = (Button) findViewById(R.id.area_learning);
        btn2 = (Button) findViewById(R.id.one_point);
        btn3 = (Button) findViewById(R.id.two_point);
        btn1.setOnClickListener(new View.OnClickListener() {
            @Override
            public void onClick(View v) {
                Intent startAreaLearning = new Intent(MainActivity.this, AreaLearningActivity.class);
                startActivity(startAreaLearning);



            }
        });

        btn2.setOnClickListener(new View.OnClickListener() {
            @Override
            public void onClick(View v) {
                Intent startOnePointDistance = new Intent(MainActivity.this, OnePointActivity.class);
                startActivity(startOnePointDistance);
            }
        });
        btn3.setOnClickListener(new View.OnClickListener() {
            @Override
            public void onClick(View v) {
                Intent startTwoPointsDistance = new Intent(MainActivity.this, TwoPointActivity.class);
                startActivity(startTwoPointsDistance);
            }

        });

    }

    /**
     * Initialize Tango Service as a normal Android Service.
     */
    @Override
    protected void onResume(){
        super.onResume();

        mTango=new Tango(MainActivity.this, new Runnable() {
            @Override
            public void run() {
                synchronized (MainActivity.this){
                    try{
                        mConfig=setTangoConfig(mTango);
                        mTango.connect(mConfig);
                        startupTango();
                    }catch(TangoOutOfDateException e){
                        Log.e(TAG,getString(R.string.exception_out_of_date),e);
                        showsToastAndFinishOnUiThread(R.string.exception_out_of_date);
                    }catch (TangoErrorException e){
                        Log.e(TAG,getString(R.string.exception_tango_error),e);
                        showsToastAndFinishOnUiThread(R.string.exception_tango_error);
                    }catch(TangoInvalidException e){
                        Log.e(TAG,getString(R.string.exception_tango_invalid),e);
                        showsToastAndFinishOnUiThread(R.string.exception_tango_invalid);
                    }catch (SecurityException e){
                        Log.e(TAG,getString(R.string.no_permissions),e);
                        showsToastAndFinishOnUiThread(R.string.no_permissions);
                    }
                }

                runOnUiThread(new Runnable() {
                    @Override
                    public void run() {
                        setupTextViews(mTango);
                    }
                });
            }
        });
    }

    /**
     * Disconnect the Tango device.
     */
    @Override
    protected void onPause(){
        super.onPause();
        synchronized (this){
            try{
                mTango.disconnect();
            }catch (TangoErrorException e){
                Log.e(TAG,getString(R.string.exception_tango_error),e);
            }
        }
    }

    private void setupTextViews(Tango tango){
        mUuidTextView=(TextView)findViewById(R.id.adf_uuid_textview);
        mRelocalizationTextView=(TextView)findViewById(R.id.relocalization_textview);

        ArrayList<String> fullUuidList;
        fullUuidList=tango.listAreaDescriptions();
        if(fullUuidList.size()==0){
            mUuidTextView.setText(R.string.no_uuid);
        }else {
            mUuidTextView.setText(getString(R.string.number_of_adfs)+fullUuidList.size()
                    +getString(R.string.latest_adf_is)
                    +fullUuidList.get(fullUuidList.size()-1));
        }
    }

    /**
     * Sets up the tango configuration object. Make sure mTango object is initialized before
     * making this call
     * @param tango Create a new Tango Configuration to return it to mTango
     * @return
     */
    private TangoConfig setTangoConfig(Tango tango){
        TangoConfig config=tango.getConfig(TangoConfig.CONFIG_TYPE_DEFAULT);

        ArrayList<String> fullUuidList;
        fullUuidList=tango.listAreaDescriptions();
        if (fullUuidList.size()>0){
            config.putString(TangoConfig.KEY_STRING_AREADESCRIPTION,
                    fullUuidList.get(fullUuidList.size()-1));
        }
        return config;
    }

    /**
     * Set up the callback listeners for the Tango Service and obtain other parameters required
     * after Tango Connection.
     * Listen to new Pose data.
     */
    private void startupTango(){

        final ArrayList<TangoCoordinateFramePair> framePairs=
                new ArrayList<TangoCoordinateFramePair>();

        framePairs.add(new TangoCoordinateFramePair(
                TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE,
                TangoPoseData.COORDINATE_FRAME_DEVICE));
        framePairs.add(new TangoCoordinateFramePair(
                TangoPoseData.COORDINATE_FRAME_AREA_DESCRIPTION,
                TangoPoseData.COORDINATE_FRAME_DEVICE));
        framePairs.add(new TangoCoordinateFramePair(
                TangoPoseData.COORDINATE_FRAME_AREA_DESCRIPTION,
                TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE));

        // Listen for new Tango data.
        mTango.connectListener(framePairs, new Tango.OnTangoUpdateListener() {
            @Override
            public void onPoseAvailable(final TangoPoseData pose) {
                synchronized (mSharedLock){
                    if(pose.baseFrame==TangoPoseData.COORDINATE_FRAME_AREA_DESCRIPTION
                            &&pose.targetFrame==TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE){
                        if(pose.statusCode==TangoPoseData.POSE_VALID){
                            mIsRelocalized=true;
                        }else {
                            mIsRelocalized=false;
                        }
                    }
                }

                final double deltaTime=(pose.timestamp-mPreviousPoseTimeStamp)*
                        SECS_TO_MILLISECS;
                mPreviousPoseTimeStamp=pose.timestamp;
                mTimeToNextUpdate-=deltaTime;

                if(mTimeToNextUpdate<0.0){
                    mTimeToNextUpdate=UPDATE_INTERVAL_MS;

                    runOnUiThread(new Runnable() {
                        @Override
                        public void run() {
                            synchronized (mSharedLock){
                                StringBuilder stringBuilder=new StringBuilder();

                                if(mIsRelocalized){
                                    float translation[]=pose.getTranslationAsFloats();
                                    float orientation[]=pose.getRotationAsFloats();
                                    stringBuilder.append(getString(R.string.localized)+
                                            " "+"Position: "+translation[0]+", "+translation[1]+
                                            ", "+translation[2]+". Orientation: "+orientation[0]+
                                            ", "+orientation[1]+", "+orientation[2]+", "+
                                            orientation[3]);
                                }else{
                                    stringBuilder.append(getString(R.string.not_localized));
                                }

                                mRelocalizationTextView.setText(stringBuilder.toString());
                            }
                        }
                    });
                }
            }

            @Override
            public void onXyzIjAvailable(TangoXyzIjData xyzIj){

            }

            @Override
            public void onPointCloudAvailable(TangoPointCloudData pointCloud){

            }

            @Override
            public void onTangoEvent(final TangoEvent event){

            }

            @Override
            public void onFrameAvailable(int cameraId){

            }
        });
    }

    /**
     * Display toast on UI thread.
     */
    private void showsToastAndFinishOnUiThread(final int resId){
        runOnUiThread(new Runnable() {
            @Override
            public void run() {
                Toast.makeText(MainActivity.this,
                        getString(resId),Toast.LENGTH_LONG).show();
                finish();
            }
        });
    }
}
